//
//  Camera.c
//  Vector_Math
//
//  Created by John R Peterson on 3/14/12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#include <stdio.h>
#include "Camera.h"


/* estimated camera calibration matrix 
 * f = 583 or 596 about that in units of pixels per meter
 * [   583   0   648    0]
 * [    0   583  480    0]
 * [    0    0    0     1]
 */

// TODO undistort a pixel, go from distored image coordinates to 
// undistorted image coordinates
Vec *Camera_undistort(Vec *p) {
    
}

// TODO project point from image coordinates into 3D coordinates
// converts a pixel in image coordinates to local 3D coordinates by 
// intersecting the ray with the ground plane

// remember that x image coordinate are x downwards and y to the right
// z back into the camera
Vec *Camera_intersectGround(Vec *p) {
    float f = 583;
    float cx = 480;   // center of the image in pixels
    float cy = 648;   // may need to modify these numbers when the image is downsampled
    float cameraheight = 0.205; // height of the optical axis above the ground plane
                                // in meters
    
    float k[] = {f, 0, cx, 0, f, cy, 0, 0, 1};
    Matrix *K = Matrix_createV(k, 3, 3);
    
    Matrix *invK = Matrix_inverse(K);
    
    Vec *np = Matrix_multiplyAV(invK,Vec_toHI(p)); // compute normalized image coordinate
    // should output a length 3 vector
    
    // apply rigid body transformation to transform vector from
    // local normalized image coordinates to local world coordinates
    // x forwards, y to the left, z up    
    // when composing rotations, rotations about current frame post multiply
    // then apply translation in local coordinates
    Matrix *R = Matrix_multiplyABDD(Matrix_rotateY(PI/2.0),Matrix_rotateX(PI));
    
    // apply rotation need to go from 3D to 3D homogeneous 
    Vec *nlw = Matrix_multiplyAV(R,Vec_toHI(np));
    
    // scale the vector by this value and then pull off the x and y components
    float sf = cameraheight/(nlw->val[2]);
    Vec_scaleI(nlw,sf);
    
    float groundcord[] = {nlw->val[0], nlw->val[1]};
    Vec *groundp =  Vec_create(groundcord, 2);
    
    // cleanup
    Matrix_destroy(K);
    Matrix_destroy(invK);
    Matrix_destroy(R);
    Vec_destroy(np);
    Vec_destroy(nlw);
    
    return groundp;
}

VecList* Camera_intersectGroundMany(VecList *vlist) {
    float f = 583;
    float cx = 480;   // center of the image in pixels
    float cy = 648;   // may need to modify these numbers when the image is downsampled
    float cameraheight = 0.205; // height of the optical axis above the ground plane
    // in meters
    
    float k[] = {f, 0, cx, 0, f, cy, 0, 0, 1};
    Matrix *K = Matrix_createV(k, 3, 3);
    
    Matrix *invK = Matrix_inverse(K);
    // should output a length 3 vector
    
    // apply rigid body transformation to transform vector from
    // local normalized image coordinates to local world coordinates
    // x forwards, y to the left, z up    
    // when composing rotations, rotations about current frame post multiply
    // then apply translation in local coordinates
    Matrix *R = Matrix_multiplyABDD(Matrix_rotateY(PI/2.0),Matrix_rotateX(PI));
    
    int i;
    VecList *groundpoints = VecList_createS(vlist->size);
    for (i = 0; i < vlist->size; i++) {

        Vec *np = Matrix_multiplyAV(invK,Vec_toHI(vlist->list[i])); // compute normalized image coordinate
        // apply rotation need to go from 3D to 3D homogeneous 
        Vec *nlw = Matrix_multiplyAV(R,Vec_toHI(np));
        // scale the vector by this value and then pull off the x and y components
        float sf = cameraheight/(nlw->val[2]);
        Vec_scaleI(nlw,sf);
        // convert 
        float groundcord[] = {nlw->val[0], nlw->val[1]};
        Vec *groundp =  Vec_create(groundcord, 2);
        VecList_add(groundpoints,groundp);
        
        // clean up some
        Vec_destroy(np);
        Vec_destroy(nlw);
    }
    
    // cleanup
    Matrix_destroy(K);
    Matrix_destroy(invK);
    Matrix_destroy(R);
    
    return groundpoints;
}